/**
*  \file control_precharge.c
*
*  \brief
*
*  \author Michael J. Hohmann  <michael.hohmann@linde-mh.de>
*
*
*/

/*===============================================[ private includes  ]================================================*/
#include "control_precharge.h"
#include "../assertor/assertor.h"
#include "../pal/pal_mem.h"
#include "../mlib/middle.h"
#include "../app/app.h"
#include "../output/output.h"
/*===============================================[ private defines   ]================================================*/


/*===============================================[ private datatypes ]================================================*/

/*===============================================[ private variables ]================================================*/

S16 key_state;               //key state 
U16 StatCharge;              // indicates state of charging sequence  
                             // possible values of StaCharge
#define WAIT        0        // wait for security relais to be closed  
#define TEST        1        // check initial difference between batVol and ucapVol 
#define CHARGE      2        // charge capacitors 
#define DISCHARGE   3        // discharge capacitors 
#define LOADED      4        // capacitors regularly loaded 

#define PRE_MIN_VOL 35       // Minimum allowable charge voltage
#define DIF_VOL     5  

F32 UCond_old;               // the last capactors voltage
S16 bIntCDis;                // open the contactor


struct  chargeTimer
{
	S16 cntInit;            // init   time  counter
	S16 cntTest;
	S16 cntCharge_1;         //
	S16 cntCharge_2;
	S16 cntCHA_ERR;         // slope timer	
	S16 cntDischarge;       // discharge timer
	S16 cntLOADED;          // LOADED  timer 
	S16 cntDisCheck;       // time counter  
};

BO CMAIN_flag;

static struct chargeTimer	cT;  //timer structer
 

/*===============================================[ private functions ]================================================*/
void precharge_func(F32 ucapVol,F32 batVol);
void keyoff_discharge();
void wait( F32 ucapVol,F32 batVol);
void test(F32 ucapVol,F32 batVol);
void charge(F32 ucapVol,F32 batVol);
void discharge(F32 ucapVol,F32 batVol);
void loaded(F32 ucapVol,F32 batVol);
/*================================================[ inline functions ]================================================*/

/*================================================[ public functions ]================================================*/
RC control_precharge_initialize( void )
{
   
   StatCharge     = WAIT;
   UCond_old      = 0;
   bIntCDis       = 0;
   CMAIN_flag     = 0;
   key_state      = 1;  //key_off = 0, key_on = 1 restart_key_on = 2
   
   pal_memzero( &cT, sizeof(cT) );
   pal_memzero( &pR, sizeof(pR) );
   return RC_SUCCESS;
}

/******************************************************************
*
*    function: precharge_10ms()
*
*       input:  U32 ucapVol ---  Capacitor Voltage  dc_link_voltage
*               U32 batVol  ---  safety relay outpout voltage (key_switch)
*
***************************************************************************/
BO precharge_10ms(F32 ucapVol,F32 batVol)
{
	output_data.v_debug_2[0] = key_state;
	output_data.v_debug_2[1] = StatCharge;


	static BO rv;
	if((batVol > (F32)5)&&((S16)0 == key_state))
	{
        key_state = 2;
	}
	else
	{
	}
	if(2 == key_state)
	{
		StatCharge   = WAIT;
		key_state    = 1;  
	}
	else
	{
	}
    precharge_func(ucapVol,batVol);
    keyoff_discharge(batVol);

	if( 1 == pR.start_charge)
	{
        rv = 1;
	}
	else
	{
        rv = 0;
	}
	return rv;
}

void precharge_func(F32 ucapVol,F32 batVol)
{

	cT.cntInit = middle_s16( 0, cT.cntInit + 10, 10000);
	
	switch (StatCharge)
	{
	    case WAIT: 
		    wait(ucapVol,batVol);
			break;
	    case TEST:		  
			test(ucapVol,batVol);
			break;
	    case CHARGE:	  
	  	    charge(ucapVol,batVol);
			break;  
	    case DISCHARGE: 
            discharge(ucapVol,batVol);
			break; 	 
	    case LOADED:
	  	    loaded(ucapVol,batVol);
	        break;
		default:
		    break;
	}
}

void keyoff_discharge()
{
	if(pR.discharge_flag == 1)
	{ 
		dis_ctrl.trac_PwmEna      = 1;
	    dis_ctrl.trac_CmdEnable   = 1;
		dis_ctrl.trac_discharge   = 1; 

    	dis_ctrl.pump_pwmEnabl    = 1;
	    dis_ctrl.pump_Enable      = 1; 
	    dis_ctrl.pump_motor_discharge = 1;
	   
	}
	else
	{
		dis_ctrl.trac_PwmEna 	 = 0;
		dis_ctrl.trac_CmdEnable	 = 0;
		dis_ctrl.trac_discharge	 = 0; 
		
		dis_ctrl.pump_pwmEnabl	 = 0;
		dis_ctrl.pump_Enable 	 = 0; 
		dis_ctrl.pump_motor_discharge = 0;

	}
}
void wait(F32 ucapVol,F32 batVol)
{
	  UCond_old = ucapVol;			                               // save actual capacitor voltages
	  if (batVol >= (F32)PRE_MIN_VOL)
	  {			   	
		   StatCharge = TEST;			                               // Relay probably closed, batVol = UBatt  35v
	  }
}
void test(F32 ucapVol,F32 batVol)
{
	if((ucapVol - batVol) > (F32)5)                                  // testing of voltages to see where to start from    ucapVol is 5V or more above batVol(battery) - wiring error
     {                                     
        cT.cntTest = middle_s16( 0, cT.cntTest + 10, 50 );    // time counter   50ms    time delay judge
        if( cT.cntTest >= 50 )	
        {
			cT.cntTest   = 0;
            bK1wireError = 1;                                  // signal wire error
        }
    }
    else if ( ucapVol < (batVol * 0.8f ))            // ucapVol is 5V or more below batVol(battery)
	{ 					                                      
		cT.cntTest = middle_s16 (0, cT.cntTest + 10, 50);       //delay  50ms     
       	if (cT.cntTest >= 50)							        // after 50ms testing make the decision   
	    {
			StatCharge = CHARGE;	                            //  to charging
			cT.cntTest= 0;
		}
		else
		{
			/* code */
		}
	}
	else                                                         // ucapVol > batVol *0.8  
	{
		cT.cntTest = middle_s16 (0, cT.cntTest + 10, 50);  
		if (cT.cntTest >= 50)						               // ucapVol is within 5V of Urel(battery) for 400ms
	    {
		    StatCharge = DISCHARGE;					               // need to discharge caps 					
			cT.cntTest = 0;
	    }
		else
		{
			/* code */
		}
	}

}
void charge(F32 ucapVol,F32 batVol)
{
    pR.start_charge = 1;				  		     		       /* charging capacitors */  
	cT.cntCharge_1 = middle_s16 (0, cT.cntCharge_1 + 10, 4000);    /* charge timer   */
    if((cT.cntInit >= 4000) || (cT.cntCHA_ERR >= 30)||( cT.cntCharge_1 >= 4000 ))    /* 8V <= (batVol - ucapVol) after 1s or charging  3000ms  or err >=30ms  */
    {												                // illegal slope of charging curve
	    pR.start_charge = 0;                                        /* stop charging */
		pError.charge_error =1;
        /*
		gDTCbuf[0] = BATTV;
		gDTCbuf[1] = UCond ; 			                         
		gDTCbuf[2] = UCond-UCond_old ;	                            			
		gDTCbuf[3] = cntInit ;		 	                            	
		gDTCbuf[4] = cntCHA_ERR ;		                            	
	    ErrCheck(DTC_UBATTI,1);                                   
		*/
	}  
    else if(((ucapVol-UCond_old) > (F32)5) || ((batVol - ucapVol) <	(F32)2 ))  // slope of cap voltage too high during charge value of voltage too high
    {
		cT.cntCHA_ERR = middle_s16 (0, cT.cntCHA_ERR + 10, 30);       // 30ms charge error
	}
	else if( ucapVol >= (batVol*0.9f) )                                  //(batVol - ucapVol < 8)  (batVol-ucapVol) < 8V
	{
	    cT.cntCHA_ERR = middle_s16 (0, cT.cntCHA_ERR-10, 30);
		cT.cntCharge_2 = middle_s16 (0, cT.cntCharge_2 + 10, 50);  
        if ( cT.cntCharge_2 >= 50 )                                  /*when the batVol - ucapVol < 8V, delay 50ms,go into LOADED  */
		{
			StatCharge = LOADED;                                      //charge completed  into loaded  status.
			cT.cntCharge_1 = 0;
			cT.cntCharge_2 = 0;
		    cT.cntCHA_ERR = 0;
	    }
		else
		{
			/* code */
		}
	}
	else	                                                       
	{
	  	cT.cntCHA_ERR = middle_s16 (0,cT.cntCHA_ERR-10, 30);
	}
	UCond_old = ucapVol;			// save actual capacitor voltage

}
void discharge(F32 ucapVol,F32 batVol)
{
    if(batVol < (F32)5)              //when switch off discharge 
    {  
        pR.start_charge     = 0;
		pR.complete_charge  = 0;
      	pR.discharge_flag   = 1;
		if( ucapVol< (F32)36 )
		{
			pR.start_charge     = 0;
			pR.complete_charge  = 0;
       		pR.discharge_flag   = 0;
		}
    }
	else     /* 600ms wait for discharge. cant send torque pulse until LAC ready (330ms from point where discharge required - by CANalyser measurement)  */
	{  
		if(cT.cntDischarge >= 3000)
	    {
  	        pError.discharge_error = 1;				                         //ErrCheck(DTC_CMNCHI,1);   KWP code
	    } 
		else
		{
			cT.cntDischarge = middle_s16 (0, cT.cntDischarge + 10, 3000);     // wait for 3000ms
			if((batVol - ucapVol )< (F32)5 )     		                          // (batVol - ucapVol) < 5V
            {	
	            pR.start_charge     = 0;
                pR.complete_charge  = 0;
			    pR.discharge_flag   = 1;	                                   // discharge
		    }	
		    else                                                               // (batVol - ucapVol) >= 5v
		    {	
			    if(ucapVol < (batVol * 0.8))
			    {  
				    cT.cntDisCheck = middle_s16 (0, cT.cntDisCheck + 10, 50);   //delay  50ms	
					if(cT.cntDisCheck >=50 )
			        {
                        StatCharge        = CHARGE;                            //   > 8v go into charge  
                        pR.discharge_flag = 0;                                 // stop discharge
                        cT.cntDischarge   = 0;                                 /* timer clear zero */
                        cT.cntDisCheck    = 0;           
					}
					else
					{
						/* code */
					}
			    }
				else
				{
					/* code */
				}
				
			}
		}	
	}
}
void loaded(F32 ucapVol,F32 batVol)
{
	if(cT.cntLOADED >= 100)                                            
	{
	    pR.start_charge= 0;
		pR.complete_charge= 1;  	                                   /* open the main contactor  */
		cT.cntInit  = 0;
	}
	else
	{
		cT.cntLOADED = middle_s16 (0, cT.cntLOADED + 10, 100);         /*  delay 1000ms  open the  main contactor */ 
	}
	if((batVol - ucapVol)>(F32)9 )                                                 
	{ 
	    cT.cntLOADED       = 0;
		cT.cntInit         = 0;
	    UCond_old          =  ucapVol;	                                    
	    pR.complete_charge = 0;
		StatCharge         = CHARGE;
	}
	if(batVol <(F32)5)
	{
	    key_state  = 0;  //key switch off
		StatCharge = DISCHARGE;
	}
	else
	{
	}
}
/*===============================================[ private functions ]================================================*/


/***********************************************************************************************************************
*                             Copyright 2020 Linde Material Handling. All rights reserved.                             *
***********************************************************************************************************************/
